package ar.edu.untref.robotica.simulador;

import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.event.MouseWheelEvent;
import java.awt.event.MouseWheelListener;
import java.math.BigDecimal;
import java.util.Timer;
import java.util.TimerTask;

import ar.edu.untref.robotica.motorfisico.Calculadora;

public class Simulador extends TimerTask implements KeyListener,
MouseWheelListener {

	private int fps = 60;
	private Graficador graficador;
	private Escenario escenario;
	private Robot robot;
	private double velocidadFinalDeseadaIzquierda = 0;
	private double velocidadFinalDeseadaDerecha = 0;
	private double fuerzaAceleracion = 0.05;
	private GraficadorSlick graficadorSlick;

	public Simulador(Escenario escenario) {

		this.escenario = escenario;
	}

	public void inicializar() {

		this.graficador = new Graficador(this.escenario);
		this.graficador.addKeyListener(this);
		this.graficador.addMouseWheelListener(this);
		this.graficador.setFocusable(true); // Para que ande el teclado.
		this.robot = new Robot(Configuracion.instancia().getAltoRobot(),
				Configuracion.instancia().getAnchoRobot());
		Timer timer = new Timer();
		double period = (double) 1 / this.fps * 1000;
		timer.scheduleAtFixedRate(this, 0, (long) period);
		// Graficador OpenGL
		graficadorSlick = new GraficadorSlick(this.escenario);
	}

	@Override
	public void keyPressed(KeyEvent e) {

		switch (e.getKeyChar()) {
		case 'w':
			acelerarRobot(1, 1);
			break;
		case 'x':
			acelerarRobot(0, 0);
			break;
		case 's':
			acelerarRobot(-1, -1);
			break;
		case 'd':
			acelerarRobot(+1, -1);
			break;
		case 'a':
			acelerarRobot(-1, +1);
			break;
		case 'g':
			acelerarRobot(this.velocidadFinalDeseadaIzquierda + 1,
					this.velocidadFinalDeseadaDerecha);
			break;
		case 'b':
			acelerarRobot(this.velocidadFinalDeseadaIzquierda - 1,
					this.velocidadFinalDeseadaDerecha);
			break;
		case 'h':
			acelerarRobot(this.velocidadFinalDeseadaIzquierda,
					this.velocidadFinalDeseadaDerecha + 1);
			break;
		case 'n':
			acelerarRobot(this.velocidadFinalDeseadaIzquierda,
					this.velocidadFinalDeseadaDerecha - 1);
			break;
		case '+':
			this.graficador.aumentarZoom();
			break;
		case '-':
			this.graficador.disminuirZoom();
			break;
		case 'i':
			System.out.println("Obstaculo a "
					+ this.robot.getDistanciaObstaculoAdelante(this.escenario));
			break;
		case 'k':
			System.out.println("Obstaculo a "
					+ this.robot.getDistanciaObstaculoAtras(this.escenario));
			break;
		case 'j':
			System.out.println("Obstaculo a "
					+ this.robot
					.getDistanciaObstaculo45Izquierda(this.escenario));
			break;
		case 'l':
			System.out
			.println("Obstaculo a "
					+ this.robot
					.getDistanciaObstaculo45Derecha(this.escenario));
			break;
		}
	}

	@Override
	public void keyTyped(KeyEvent e) {

		/* nada que hacer acá */
	}

	@Override
	public void keyReleased(KeyEvent e) {

		/* nada que hacer acá */
	}

	@Override
	public void run() {

		if (this.robot.getVelocidadDesplazamiento() > 0) {
			if (!puedeAvanzar()) {
				acelerarRobot(0, 0);
			}
		} else if (this.robot.getVelocidadDesplazamiento() < 0) {
			if (!puedeRetroceder()) {
				acelerarRobot(0, 0);
			}
		}
		this.escenario.aumentarAngulo(Calculadora.getCambioAngulo(
				this.robot.getRuedaI(), this.robot.getRuedaD())
				/ this.fps);
		logicaAceleracion();
		reflejarMovimiento();
	}

	public boolean puedeAvanzar() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAdelante(this.escenario) > distanciaSegura
				|| this.robot.getDistanciaObstaculo45Derecha(this.escenario) > distanciaSegura
				|| this.robot.getDistanciaObstaculo45Izquierda(this.escenario) > distanciaSegura;
	}

	public boolean puedeRetroceder() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAtras(this.escenario) > distanciaSegura;
	}

	public void reflejarMovimiento() {

		Posicion posicionRobot = this.escenario.getPosicionRobot();
		double posicionXActual = posicionRobot.getPosicionX();
		double posicionYActual = posicionRobot.getPosicionY();
		double velocidad = this.robot.getVelocidadDesplazamiento();
		double angulo = Math.toRadians(this.escenario.getPosicionRobot()
				.getAngulo());
		double posicionXNueva = posicionXActual + velocidad * Math.sin(angulo);
		double posicionYNueva = posicionYActual - velocidad * Math.cos(angulo);
		posicionRobot.setPosicionX(posicionXNueva);
		posicionRobot.setPosicionY(posicionYNueva);
		this.graficador.repaint();
	}

	private void logicaAceleracion() {

		BigDecimal deltaDeAceleracion = new BigDecimal(this.fuerzaAceleracion);
		if (!velocidadDeseadaAlcanzadaIzquierda()) {
			if (this.velocidadFinalDeseadaIzquierda > this.robot.getRuedaI()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
						.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaIzquierda < this.robot
					.getRuedaI().getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
						.subtract(deltaDeAceleracion));
			}
		}
		if (!velocidadDeseadaAlcanzadaDerecha()) {
			if (this.velocidadFinalDeseadaDerecha > this.robot.getRuedaD()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
						.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaDerecha < this.robot
					.getRuedaD().getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
						.subtract(deltaDeAceleracion));
			}
		}
	}

	public void acelerarRobot(double velocidadFinalIzquierda,
			double velocidadFinalDerecha) {

		this.velocidadFinalDeseadaIzquierda = velocidadFinalIzquierda;
		this.velocidadFinalDeseadaDerecha = velocidadFinalDerecha;
	}

	private boolean velocidadDeseadaAlcanzadaIzquierda() {

		return this.robot.getRuedaI().getVelocidad().doubleValue() == this.velocidadFinalDeseadaIzquierda;
	}

	private boolean velocidadDeseadaAlcanzadaDerecha() {

		return this.robot.getRuedaD().getVelocidad().doubleValue() == this.velocidadFinalDeseadaDerecha;
	}

	@Override
	public void mouseWheelMoved(MouseWheelEvent e) {

		int notches = e.getWheelRotation();
		if (notches < 0) {
			this.graficador.aumentarZoom();
		} else {
			this.graficador.disminuirZoom();
		}
	}
}
